#include "mbed.h"
#include "m3pi.h"
#include "XBeeSerial.h"

using namespace std;

m3pi pi;
Ticker ticker;

Serial pc(USBTX, USBRX);
XBeeSerial xb; // Uses p28 and p27 by default

void handleRXResponse(XBeeResponse* xbr) {
    pc.printf("Got something... \r\n");
    
    if(xbr->address16 > 1) {
        pc.printf("Got: %i\r\n", xbr->msgData[1]);
        
        xb.sendTX(0x44, xbr->address16, "p", 1);
    }
    else {
        pc.printf("Got from controller: %i\r\n", xbr->msgData[1]);
        //xb.sendTX(0x44, 0x01, "p", 1);
    }
}

int main() {

    // This allows us to send messages to a Serial object from within the XBeeSerial class
    xb.setDebugger(&pc);
    
    xb.baud(38400);
    pc.baud(38400);
    
    xb.sendLocalMessage(0x08, 0x52, "BD");
    
    xb.attachHandler(RESP_RX_16BIT, handleRXResponse);
    
    xb.sendTX(0x44, 0x02, "p", 1);
    xb.sendTX(0x44, 0x03, "p", 1);
    xb.sendTX(0x44, 0x04, "p", 1);
    
    while(1) {
        
    }
}
